/*
 * @Author: LVGRAPE
 * @Date: 2023-09-06 10:22:03
 * @LastEditTime: 2024-05-15 11:10:23
 * @LastEditors: lvgrape lvgrape@outlook.com
 * @Description:
 * @FilePath: \ZINO_FC_V4\ZINO\hardware\pwr\voltage.c
 * 可以输入预定的版权声明、个性签名、空行等
 */
#include "drv_common.h"
#if defined(BSP_USING_ADC1) && defined(RT_USING_ADC)

#include <rtdevice.h>
#include "drv_gpio.h"
#include "zino.h"
#include "pin.h"
#include "rc.h"
#include "filter.h"
#include "rgb.h"

#include <stdbool.h>
#define ADC_DEV_NAME "adc1"
#define DBG_TAG "vol"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#define CHARGE_PIN GET_PIN(B, 4)
#define STANDBY_PIN GET_PIN(B, 5)
#define VBM_PIN GET_PIN(B, 15)
#define V25_MV 1280     // 在25 ℃时的电压1.28V
#define AVG_SLOPE -4.2f // 平均斜率 -4.20Mv/℃
enum zino_vol_index
{
    V_INDEX_VBAT = 0, // battery
    V_INDEX_CH1 = 1,  // sticks
    V_INDEX_CH2 = 2,  // sticks
    V_INDEX_CH3 = 3,  // sticks
    V_INDEX_CH4 = 4,  // sticks
    V_INDEX_CH5 = 10, // button
    V_INDEX_CH6 = 11, // button
    V_INDEX_CH7 = 12, // button
    V_INDEX_CH8 = 13, // button
    V_INDEX_COUNT = 9,

};
struct zino_voltage
{
    union voltage
    {
        struct
        {
            uint16_t vbat;
            uint16_t ch[V_INDEX_COUNT - 1];
        };
        uint16_t raw[V_INDEX_COUNT];
    } adc;

    int16_t vbat_mv;
    int16_t ch[V_INDEX_COUNT - 1];
} __PACKED zinoVol;
Kalman_t kf[V_INDEX_COUNT];
bool printon = 0;
const rt_uint8_t adc_list[] = {
    // NOTE 电压采集顺序不能变，需和zinoVol.adc对应
    V_INDEX_VBAT,
    V_INDEX_CH1,
    V_INDEX_CH2,
    V_INDEX_CH3,
    V_INDEX_CH4,
    V_INDEX_CH5,
    V_INDEX_CH6,
    V_INDEX_CH7,
    V_INDEX_CH8,
};
void voltage_cal(struct zino_voltage *v, enum zino_vol_index ch)
{

    if (ch > V_INDEX_COUNT)
        return;
    if (ch == V_INDEX_VBAT)
        v->vbat_mv = KalmanFilter(&kf[ch], (float)v->adc.raw[ch] * 1.645f); // v->adc.raw[ch] * 3300 / 4096 * 2;
    else
        // v->ch[ch - 1] = KalmanFilter(&kf[ch], (float)v->adc.raw[ch] * 0.48828125f); //[0,2000], v->adc.raw[ch] / 4096 * 2000;
        v->ch[ch - 1] = (float)v->adc.raw[ch] * 0.48828125f;
}
union sw_dOs
{
    uint8_t c[4];
    float f;
    int16_t __i16;
    int32_t __i32;
    uint16_t __u16;
    uint32_t __u32;
} sw_data;
typedef enum
{
    SW_TYPE_i8 = 0,
    SW_TYPE_u8,
    SW_TYPE_i16,
    SW_TYPE_u16,
    SW_TYPE_i32,
    SW_TYPE_u32,
    SW_TYPE_f4,
} SW_TYPE_E;
void sw_digitalOsc_out(SW_TYPE_E type, void *buf, uint8_t count)
{
    if (!count || !buf)
        return;
    uint8_t buffer[36];
    uint8_t bw = 1;
    buffer[0] = 0x03;
    buffer[1] = 0xfc;
    if (count > 8)
        count = 8;
    switch (type)
    {
    case SW_TYPE_i8:
    case SW_TYPE_u8:
        bw = 1;
        break;
    case SW_TYPE_i16:
    case SW_TYPE_u16:
        bw = 2;
        break;
    case SW_TYPE_i32:
    case SW_TYPE_u32:
        bw = 4;
        break;
    case SW_TYPE_f4:
        bw = 4;
        break;
    default:
        break;
    }
    memcpy(&buffer[2], buf, count * bw);
    buffer[2 + count * bw] = 0xfc;
    buffer[2 + count * bw + 1] = 0x03;
    rt_kput(buffer, count * bw + 4);
}
void voltage_mon_task(void *args)
{
    rt_adc_device_t adc_device = RT_NULL;
    adc_device = (rt_adc_device_t)rt_device_find(ADC_DEV_NAME);
    const rt_uint8_t chs_cnt = sizeof(adc_list) / sizeof(adc_list[0]);
    rt_uint8_t chs_cur = 0;
    if (adc_device == RT_NULL)
    {
        LOG_E("adc1 not found!");
        return;
    }
    LOG_D("chs_cnt:%d", chs_cnt);
    rt_adc_enable(adc_device, adc_list[chs_cur]);
    // rt_pin_mode(VBM_PIN, PIN_MODE_OUTPUT_OD);
    // rt_pin_write(VBM_PIN, 0);
    KalmanFilterInitParam(&kf[0], 0.01, 10);
    // for (uint8_t i = 1; i < V_INDEX_COUNT; i++)
    // {
    //     KalmanFilterInitParam(&kf[i], 0.1, 10);
    // }
    KalmanFilterInitParam(&kf[1], 1, 2);
    KalmanFilterInitParam(&kf[2], 1, 2);
    KalmanFilterInitParam(&kf[3], 1, 2);
    KalmanFilterInitParam(&kf[4], 1, 2);
    KalmanFilterInitParam(&kf[5], 0.1, 1);
    KalmanFilterInitParam(&kf[6], 0.1, 1);
    KalmanFilterInitParam(&kf[7], 0.1, 1);
    KalmanFilterInitParam(&kf[8], 0.1, 1);

    rt_pin_mode(CHARGE_PIN, PIN_MODE_INPUT);
    rt_pin_mode(STANDBY_PIN, PIN_MODE_INPUT);

    const rt_uint8_t s12_pin = GET_PIN(A, 11);
    const rt_uint8_t s13_pin = GET_PIN(A, 12);
    const rt_uint8_t s14_pin = GET_PIN(A, 13);
    const rt_uint8_t s15_pin = GET_PIN(A, 14);
    const rt_uint8_t s16_pin = GET_PIN(C, 10);
    const rt_uint8_t s17_pin = GET_PIN(C, 11);
    
    rt_pin_mode(s12_pin, PIN_MODE_INPUT_PULLUP);rt_pin_write(s12_pin, PIN_HIGH);
    rt_pin_mode(s13_pin, PIN_MODE_INPUT_PULLUP);rt_pin_write(s13_pin, PIN_HIGH);
    rt_pin_mode(s14_pin, PIN_MODE_INPUT_PULLUP);rt_pin_write(s14_pin, PIN_HIGH);
    rt_pin_mode(s15_pin, PIN_MODE_INPUT_PULLUP);rt_pin_write(s15_pin, PIN_HIGH);
    rt_pin_mode(s16_pin, PIN_MODE_INPUT_PULLUP);rt_pin_write(s16_pin, PIN_HIGH);
    rt_pin_mode(s17_pin, PIN_MODE_INPUT_PULLUP);rt_pin_write(s17_pin, PIN_HIGH);
    
    uint8_t cnt = 0;
    while (1)
    {
        zinoVol.adc.raw[chs_cur] = rt_adc_read(adc_device, adc_list[chs_cur]);
        voltage_cal(&zinoVol, chs_cur);
        chs_cur++;
        if (chs_cur == chs_cnt && printon)
        {

            if (cnt++ >= 2)
            {

                // float vtemp[9];
                // vtemp[0] = zinoVol.vbat_mv;
                // for (uint8_t i = 1; i < V_INDEX_COUNT; i++)
                // {
                //     vtemp[i] = zinoVol.ch[i - 1];
                // }
                // sw_digitalOsc_out(SW_TYPE_f4, &vtemp[1], 8);
                // rt_kprintf("\n");
                // rt_kprintf("sticks:%04d %04d %04d %04d\t",
                // zinoCommonRcData.values.stick.LLR,
                // zinoCommonRcData.values.stick.LUD,
                // zinoCommonRcData.values.stick.RLR,
                // zinoCommonRcData.values.stick.RUD);
                // rt_kprintf("button:%d %d %d %d %d %d %d %d\n",
                // zinoCommonRcData.values.button.LL,
                // zinoCommonRcData.values.button.LR,
                // zinoCommonRcData.values.button.LU,
                // zinoCommonRcData.values.button.LD,
                // zinoCommonRcData.values.button.RL,
                // zinoCommonRcData.values.button.RR,
                // zinoCommonRcData.values.button.RU,
                // zinoCommonRcData.values.button.RD
                // );
                // rt_kprintf("vbat: %d %04d \t %04d \t %04d \t %04d \t %04d \t %04d \t %04d \t %04d \t %04d \n", chs_cur, (int)zinoVol.vbat_mv,zinoVol.ch[0],
                //             zinoVol.ch[1],zinoVol.ch[2],zinoVol.ch[3],
                //             zinoVol.ch[4],zinoVol.ch[5],zinoVol.ch[6],
                //             zinoVol.ch[7]);
                for(uint8_t i=2;i<12;i+=2)
                {
                    rt_kprintf("%04d\t",*(uint16_t*)&zinoCommonRcData.raw[i]);
                }
                rt_kprintf("\n");
                // rt_kprintf("sticks:%04d %04d %04d %04d\t",
                //            zinoCommonRcData.values.stick.LLR,
                //            zinoCommonRcData.values.stick.LUD,
                //            zinoCommonRcData.values.stick.RLR,
                //            zinoCommonRcData.values.stick.RUD);
                // rt_kprintf("button:%d %d %d %d %d %d %d %d\n",
                //            zinoCommonRcData.values.button.LL,
                //            zinoCommonRcData.values.button.LR,
                //            zinoCommonRcData.values.button.LU,
                //            zinoCommonRcData.values.button.LD,
                //            zinoCommonRcData.values.button.RL,
                //            zinoCommonRcData.values.button.RR,
                //            zinoCommonRcData.values.button.RU,
                //            zinoCommonRcData.values.button.RD);
                cnt = 0;
            }
        }

        if (chs_cur >= chs_cnt)
        {
            chs_cur = 0;
            zinoCommonRcData.values.stick.LLR = 2000-zinoVol.ch[0];
            zinoCommonRcData.values.stick.LUD = zinoVol.ch[1];
            zinoCommonRcData.values.stick.RLR = 2000-zinoVol.ch[2];
            zinoCommonRcData.values.stick.RUD = zinoVol.ch[3];

            zinoCommonRcData.values.button.LL = (zinoVol.ch[4] / 800 == 1);
            zinoCommonRcData.values.button.LR = (zinoVol.ch[4] / 800 == 2);
            zinoCommonRcData.values.button.LU = (zinoVol.ch[5] / 800 == 1);
            zinoCommonRcData.values.button.LD = (zinoVol.ch[5] / 800 == 2);
            zinoCommonRcData.values.button.RL = (zinoVol.ch[6] / 800 == 1);
            zinoCommonRcData.values.button.RR = (zinoVol.ch[6] / 800 == 2);
            zinoCommonRcData.values.button.RU = (zinoVol.ch[7] / 800 == 1);
            zinoCommonRcData.values.button.RD = (zinoVol.ch[7] / 800 == 2);
            zinoCommonRcData.values.button.q = !rt_pin_read(s12_pin);
            zinoCommonRcData.values.button.d = !rt_pin_read(s13_pin);
            zinoCommonRcData.values.button.b = !rt_pin_read(s14_pin);
            zinoCommonRcData.values.button.p = !rt_pin_read(s15_pin);
            zinoCommonRcData.values.button.LT = !rt_pin_read(s16_pin);
            zinoCommonRcData.values.button.RT = !rt_pin_read(s17_pin);

            uint8_t vbat_level = (zinoVol.vbat_mv - 3300) / 225 + 1;
            battery_led_set(vbat_level, rt_pin_read(CHARGE_PIN)==0);

            // struct rc_handle *rc=get_rc_channel_ram();

            // if(rc->rx_rssi>1)
            // {
            //     rgb_set(0,255,0);
            // }
            // else
            // {
            //     rgb_set(255,255,0);
            //     rt_thread_mdelay(500);
            //     rgb_set(0,0,0);
            //     rt_thread_mdelay(500);
            // }
        }
        rt_adc_enable(adc_device, adc_list[chs_cur]);
        rt_thread_mdelay(1);
    }
}

int voltage_task_launch()
{
    rt_thread_t vol_threadId = rt_thread_create("vol", voltage_mon_task, RT_NULL, 512, 25, 10);
    if (vol_threadId)
    {
        rt_thread_startup(vol_threadId);
        LOG_I("voltage_task_launch");
        return RT_EOK;
    }
    else
    {
        LOG_E("voltage_task_launch failed!");
        return RT_ERROR;
    }
    return RT_EOK;
}
// float get_vbat_voltage_mv()
// {
//     return zinoVol.vbat_mv;
// }
// float get_v5_voltage_mv()
// {
//     return zinoVol.v5_mv;
// }
// float get_v3_3_voltage_mv()
// {
//     return zinoVol.v3_3_mv;
// }
// float get_m1_current_ma()
// {
//     return zinoVol.m1_ma;
// }
// float get_m2_current_ma()
// {
//     return zinoVol.m2_ma;
// }
// float get_m3_current_ma()
// {
//     return zinoVol.m3_ma;
// }
// float get_m4_current_ma()
// {
//     return zinoVol.m4_ma;
// }

long vol(int argc, char **argv)
{
    if (argc == 1)
    {
        rt_kprintf("Vbat>%d.%03dV \t", (int)zinoVol.vbat_mv / 1000, (int)zinoVol.vbat_mv % 1000);
        for (uint8_t i = 1; i < V_INDEX_COUNT; i++)
        {
            rt_kprintf("CH%d>%d.%03dV \t", i, (int)zinoVol.ch[i - 1] / 1000, (int)zinoVol.ch[i - 1] % 1000);
        }
        rt_kprintf("\n");
    }
    else if (argc == 2)
    {
        if (rt_strcmp(argv[1], "raw") == 0)
        {

            rt_kprintf("\n");
        }
        else if (rt_strcmp(argv[1], "show") == 0)
        {
            printon = TRUE;
        }
        else if (rt_strcmp(argv[1], "close") == 0)
        {
            printon = FALSE;
        }
    }
    return RT_EOK;
}
ZINO_APP_EXPORT(voltage_task_launch);
MSH_CMD_EXPORT(vol, show system voltage);
#else
#error "please define DSP_USING_ADC1 and RT_USING_ADC"
#endif
